#include "pcpt_comm/pcpt_comm_tf.h"

/**************************************************************************************
功能描述: 设置转化关系
修改记录:
**************************************************************************************/
void POSE_TRANSFORM_C::SetTransRelationship(const COOR_2D_POSE_STRU &stSonPoseByMap, const COOR_2D_POSE_STRU &stFatherPoseByMap)
{
    float sinSonAng = sin(stSonPoseByMap.dirAngle);
    float cosSonAng = cos(stSonPoseByMap.dirAngle);
    
    float sinFatherAng = sin(stFatherPoseByMap.dirAngle);
    float cosFatherAng = cos(stFatherPoseByMap.dirAngle);

    float difX = stSonPoseByMap.stPos.x - stFatherPoseByMap.stPos.x;
    float difY = stSonPoseByMap.stPos.y - stFatherPoseByMap.stPos.y;

    coeffAX_ =  cosFatherAng * cosSonAng + sinFatherAng * sinSonAng;
    coeffBX_ = -cosFatherAng * sinSonAng + sinFatherAng * cosSonAng;
    constCX_ =  cosFatherAng * difX      + sinFatherAng * difY;

    coeffAY_ = -sinFatherAng * cosSonAng + cosFatherAng * sinSonAng;
    coeffBY_ =  sinFatherAng * sinSonAng + cosFatherAng * cosSonAng;
    constCY_ = -sinFatherAng * difX      + cosFatherAng * difY;
}

void POSE_TRANSFORM_C::SetTransRelationship(const COOR_3D_POSE_STRU &stSonPoseByMap, const COOR_3D_POSE_STRU &stFatherPoseByMap)
{
    tfSonPoseByFather_ = stFatherPoseByMap.GetTransformInv() * stSonPoseByMap.GetTransform();
}

/**************************************************************************************
功能描述: 2D坐标变化
修改记录:
**************************************************************************************/
void POSE_TRANSFORM_C::TransformPointBySon2ByFather2D(const MyPoint &pointBySon, MyPoint &pointByFather)
{
    pointByFather.x = coeffAX_ * pointBySon.x + coeffBX_ * pointBySon.y + constCX_;
    pointByFather.y = coeffAY_ * pointBySon.x + coeffBY_ * pointBySon.y + constCY_;
}

/**************************************************************************************
功能描述: 2D点云坐标变化
修改记录:
**************************************************************************************/
void POSE_TRANSFORM_C::TransformPointCloudBySon2ByFather2D(const MyCloud::Ptr pCloudBySon, MyCloud::Ptr pCloudByFather)
{
    MyPoint pointTrans;
    for (const auto &point : pCloudBySon->points) {
        TransformPointBySon2ByFather2D(point, pointTrans);
        pCloudByFather->points.push_back(pointTrans);
    }
}

/**************************************************************************************
功能描述: 3D点云坐标变化
修改记录:
**************************************************************************************/
void POSE_TRANSFORM_C::TransformPointCloudBySon2ByFather3D(const MyCloud::Ptr pCloudBySon, MyCloud::Ptr pCloudByFather)
{
    pcl::transformPointCloud(*pCloudBySon, *pCloudByFather, tfSonPoseByFather_);
}

/**************************************************************************************
功能描述: 由TF获取当前3维定位信息
修改记录:
**************************************************************************************/
INT32 POSE_TRANSFORM_C::GetLocationMsgFromTF(COOR_3D_POSE_STRU &stPose)
{
    if (GetTransMatrixFromTF(tfPoseByMap_) != 0)
        return -1;

    stPose.Copy(tfPoseByMap_);
    
    return 0;
}

/**************************************************************************************
功能描述: 由TF获取当前位姿转换矩阵
修改记录:
**************************************************************************************/
INT32 POSE_TRANSFORM_C::GetTransMatrixFromTF(Eigen::Matrix4f &tfPoseByMap)
{
    static tf::TransformListener   listenerTf_;
    static tf::StampedTransform    transform_;
    
    try {
        listenerTf_.lookupTransform("/map", "/base_link", ros::Time(0), transform_);
    } catch (tf::TransformException &ex) {
//        ST_LOG_ERR("Transform error of sensor data: %s", ex.what());
        return -1;
    }

    pcl_ros::transformAsMatrix(transform_, tfPoseByMap);

    return 0;
}

